#include "node_factory.hpp"
#include "ros/ros.h"
#include <yaml-cpp/yaml.h>
#include "data_2d2s_path.h"
#include "node_config_path.h"
using namespace auto_ros::control;
int main(int argc, char **argv)
{
	NodeFactory node_factory;
	ros::init(argc, argv, "osqp_control");
	YAML::Node main_yaml = YAML::LoadFile(node_config_path + "main.yaml");
	std::fstream fout(node_config_path + "main.yaml" + ".cor", std::ios::out);
	fout << main_yaml;
	fout.close();
	std::shared_ptr<NodeBase>
		node_ptr = node_factory.CreateSharedObject(main_yaml["node_class_name"].as<std::string>(), 50, "node.yaml");

	ROS_INFO("INIT SUSCESS");
	while (1)
	{

		node_ptr->LoopProc();
		ros::spinOnce(); //可用回调函数
		node_ptr->loop_rate_.sleep();
	}

	return 0;
}